#include "WPILib.h"

//sidecars
#define LEFTSIDECAR       2
#define RIGHTSIDECAR      1

//jaguar ports
#define LEFTMOTOR         10 //10
#define RIGHTMOTOR        10 //10
#define CONVEYORBELT      5
#define TURRETMOTOR       4
#define TOPTURRETWHEEL    8 //1
#define BOTTOMTURRETWHEEL 1  //1

//Encoder ports
#define A_PORT 2
#define B_PORT 3

#define GEARTOOTHPORT 10


//Spike ports
#define RAMPTOOLPORT      1

//relay ports
#define RINGLIGHTPORT     2

//joystick numbering
#define LEFTSTICK         2
#define RIGHTSTICK        1
#define MANIPSTICK          3

#define GAMESTICK         3

#define MIDFIRE 0.5

//Limit Switches
#define BALLDETECT 1 //need to be set
#define RIGHTLIMIT 5 //need to be set
#define LEFTLIMIT 5 //need to be set
#define RAMPTOOLLIMIT 8 //limit switch for ramp tool

#define INTAKEOPEN 1.0 //need to be set
#define INTAKECLOSE 0.0 //need to be set


//Encoder constants
#define WHEEL_RADIUS   3.0486875    //Radius of jigglybot drive wheel.
#define SPROCKET_RATIO 12.0/26.0    //Gear ratio between motor and wheel.
#define MAX_ERROR      0.001        //The maximum percent error for PID loops.

//Other constants, measurements
#define PI           3.1415926535898//pie
#define ROBO_WIDTH   21.75          //width of robot, between centers of wheels
#define TO_RAD       PI / 180.0     //convert an angle in degrees to radians
#define L_TO_R_WIDTH        54.75          //horizontal distance between centers of backboards

///// Heights used in trigonometric calculation //////
#define BOT_TO_TOP_HEIGHT       70.0           //distance between centers of vertical backboards
#define BOT_TO_MID_HEIGHT 35.0           //distance between center of bottom backboard and center of middle backboards
#define BACKBOARD_HEIGHT  18.0           //height of a single reflective rectangle
#define CAM_HEIGHT   36.667        //height of camera lense above ground

//// Heights above ground /////
#define BOTTOM_CENTER_HEIGHT 42.0           //height of center of bottom backboard
#define BOTTOM_EDGE_HEIGHT 30.0     //height of bottom edge of bottom backboard

//// Field dimensions ////
#define FIELD_WIDTH  160.1          //half of the width of the field
#define FIELD_LENGTH 280.4          //distance from backboard to edge of bridge

//// MIsc. ////
#define FOV          47.0 * TO_RAD  //field of view of M1011 IP camera (in radians)
#define FIRING_CONST 10.0
#define TURRET_ACCEL_TIME 1.0

//PID values
#define DRIVE_P 0.1
#define DRIVE_I 0.0
#define DRIVE_D 0.0
#define ROTVELOCITYPERSIGNAL 6.0 //estimated

//Intake
#define PVAVLE 7 //goes to digtialIO ports on sidecar
#define PRELAY 3 //goes to relay ports on sidecar
#define PORTOP 6 //goes to digtialIO ports on sidecar
#define PORBOT 7 //goes to digtialIO ports on sidecar
#define SOLTON 1 //goes to Soleindoid module
#define SOLTOF 3 //goes to Soleindoid module
#define SOLBON 4 //goes to Soleindoid module
#define SOLBOF 5 //goes to Soleindoid module

//uncomment below if you want to use intake
//#define INTAKE_CONTROL 1
